﻿//------------------------------------------------------------------------------
// <copyright file="MainWindow.xaml.cs" company="Microsoft">
//     Copyright (c) Microsoft Corporation.  All rights reserved.
// </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Samples.Kinect.SkeletonBasics
{
    using System.IO;
    using System.Windows;
    using System.Windows.Media;
    using Microsoft.Kinect;
    using System.Text;
    using System;
    using System.Collections.Generic;
    

    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        /// <summary>
        /// Width of output drawing
        /// </summary>
        private const float RenderWidth = 640.0f;

        /// <summary>
        /// Height of our output drawing
        /// </summary>
        private const float RenderHeight = 480.0f;

        /// <summary>
        /// Thickness of drawn joint lines
        /// </summary>
        private const double JointThickness = 3;

        /// <summary>
        /// Thickness of body center ellipse
        /// </summary>
        private const double BodyCenterThickness = 10;

        /// <summary>
        /// Thickness of clip edge rectangles
        /// </summary>
        private const double ClipBoundsThickness = 10;
       
        /// <summary>
        /// Brush used to draw skeleton center point
        /// </summary>
        private readonly Brush centerPointBrush = Brushes.Blue;

        /// <summary>
        /// Brush used for drawing joints that are currently tracked
        /// </summary>
        private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));

        /// <summary>
        /// Brush used for drawing joints that are currently inferred
        /// </summary>        
        private readonly Brush inferredJointBrush = Brushes.Yellow;

        /// <summary>
        /// Pen used for drawing bones that are currently tracked
        /// </summary>
        private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);

        /// <summary>
        /// Pen used for drawing bones that are currently inferred
        /// </summary>        
        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);

        /// <summary>
        /// Active Kinect sensor
        /// </summary>
        private KinectSensor sensor;

        /// <summary>
        /// Drawing group for skeleton rendering output
        /// </summary>
        private DrawingGroup drawingGroup;

        /// <summary>
        /// Drawing image that we will display
        /// </summary>
        private DrawingImage imageSource;

        /// <summary>
        /// Initializes a new instance of the MainWindow class.
        /// </summary>
        /// 
        Server server = new Server();
        Client client;
        GestureRecognition gestRec = new GestureRecognition();
        float oldY;

        private double elevationAngle;
        bool isCalibrated = true;


        bool recording = false;
        StreamWriter writer;
       

        public MainWindow()
        {
            server.setGestureRecognizer(gestRec);
            InitializeComponent();
        }

        /// <summary>
        /// Draws indicators to show which edges are clipping skeleton data
        /// </summary>
        /// <param name="skeleton">skeleton to draw clipping information for</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private static void RenderClippedEdges(Skeleton skeleton, DrawingContext drawingContext)
        {
            if (skeleton.ClippedEdges.HasFlag(FrameEdges.Bottom))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, RenderHeight - ClipBoundsThickness, RenderWidth, ClipBoundsThickness));
            }

            if (skeleton.ClippedEdges.HasFlag(FrameEdges.Top))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, 0, RenderWidth, ClipBoundsThickness));
            }

            if (skeleton.ClippedEdges.HasFlag(FrameEdges.Left))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, 0, ClipBoundsThickness, RenderHeight));
            }

            if (skeleton.ClippedEdges.HasFlag(FrameEdges.Right))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(RenderWidth - ClipBoundsThickness, 0, ClipBoundsThickness, RenderHeight));
            }
        }

        /// <summary>
        /// Execute startup tasks
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void WindowLoaded(object sender, RoutedEventArgs e)
        {
            // Create the drawing group we'll use for drawing
            this.drawingGroup = new DrawingGroup();

            // Create an image source that we can use in our image control
            this.imageSource = new DrawingImage(this.drawingGroup);

            // Display the drawing using our image control
            Image.Source = this.imageSource;

            // Look through all sensors and start the first connected one.
            // This requires that a Kinect is connected at the time of app startup.
            // To make your app robust against plug/unplug, 
            // it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit
            foreach (var potentialSensor in KinectSensor.KinectSensors)
            {
                if (potentialSensor.Status == KinectStatus.Connected)
                {
                    this.sensor = potentialSensor;
                    break;
                }
            }


            if (null != this.sensor)
            {
                // Turn on the skeleton stream to receive skeleton frames
                this.sensor.SkeletonStream.Enable();

                // Add an event handler to be called whenever there is new color frame data
                this.sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;

                // Start the sensor!
                try
                {
                    this.sensor.Start();

                    elevationAngle = -(this.sensor.ElevationAngle) * (float)Math.PI/180;
                }
                catch (IOException)
                {
                    this.sensor = null;
                }
            }

            if (null == this.sensor)
            {
                this.statusBarText.Text = Properties.Resources.NoKinectReady;
            }
            client = new Client();
        }

        /// <summary>
        /// Execute shutdown tasks
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void WindowClosing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            if (null != this.sensor)
            {
                this.sensor.Stop();
            }

            if (writer != null && writer.BaseStream != null)
            {
                writer.Close();
            }
        }

        /// <summary>
        /// Event handler for Kinect sensor's SkeletonFrameReady event
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            Skeleton[] skeletons = new Skeleton[0];

            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    skeletonFrame.CopySkeletonDataTo(skeletons);
                }
            }

            using (DrawingContext dc = this.drawingGroup.Open())
            {
                // Draw a transparent background to set the render size
                dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));

                if (skeletons.Length != 0)
                {
                    Skeleton targetSkel = null;
                    double dist = 10000000000;

                    foreach (Skeleton skel in skeletons) {
                        double d = skel.Position.Z;

                        if (d < dist && d != 0)
                        {
                            targetSkel = skel;
                            dist = d;
                        }
                    }


                    foreach (Skeleton skel in skeletons)
                    {
                        RenderClippedEdges(skel, dc);

                        if (skel.TrackingState == SkeletonTrackingState.Tracked && skel.Equals(targetSkel))
                        {
                            this.DrawBonesAndJoints(targetSkel, dc);
                        }
                        else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
                        {
                            dc.DrawEllipse(
                            this.centerPointBrush,
                            null,
                            this.SkeletonPointToScreen(skel.Position),
                            BodyCenterThickness,
                            BodyCenterThickness);
                        }
                    }

                    if (targetSkel == null) {
                        gestRec.clearGestures();
                    }
                }
                server.calculateJointMessage();

                // prevent drawing outside of our render area
                this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
            }
        }

        /// <summary>
        /// Draws a skeleton's bones and joints
        /// </summary>
        /// <param name="skeleton">skeleton to draw</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
        {
            // Render Torso
            this.DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
            this.DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
            this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
            this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);

            // Left Arm
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
            this.DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
            this.DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);

            // Right Arm
            this.DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
            this.DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
            this.DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);

            // Left Leg
            this.DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
            this.DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
            this.DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);

            // Right Leg
            this.DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
            this.DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
            this.DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);
 
            // Render Joints
            foreach (Joint joint in skeleton.Joints)
            {
                Brush drawBrush = null;

                if (joint.TrackingState == JointTrackingState.Tracked)
                {
                    drawBrush = this.trackedJointBrush;                    
                }
                else if (joint.TrackingState == JointTrackingState.Inferred)
                {
                    drawBrush = this.inferredJointBrush;                    
                }

                if (drawBrush != null)
                {
                    drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
                }
            }


            float[] data = this.getGestureData(skeleton);

            if (gestRec.getGesturesRecognized() > 0)
            {
                gestRec.receiveRawData(data);
            }

            if (recording)
            {
                this.recordToFile(data);
            }

            if (client != null)
            {
                String s = createBoneVectorString(skeleton);
                client.setJointPositionString(s);
            }

         //   this.findJumping(skeleton);

        }


        private void findJumping(Skeleton skel) {

            float newY = skel.Joints[JointType.Spine].Position.Y;
            if (!oldY.Equals(null))
            {
                float diff = newY - oldY;
                if (diff > 0.05)
                {
                    Console.WriteLine("jumping " + diff);
                    Server.setJump();
                }
            }

            oldY = newY;
        
        }

        /// <summary>
        /// Maps a SkeletonPoint to lie within our render space and converts to Point
        /// </summary>
        /// <param name="skelpoint">point to map</param>
        /// <returns>mapped point</returns>
        private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
        {
            // Convert point to depth space.  
            // We are not using depth directly, but we do want the points in our 640x480 output resolution.
            DepthImagePoint depthPoint = this.sensor.MapSkeletonPointToDepth(
                                                                             skelpoint,
                                                                             DepthImageFormat.Resolution640x480Fps30);
            return new Point(depthPoint.X, depthPoint.Y);
        }

        /// <summary>
        /// Draws a bone line between two joints
        /// </summary>
        /// <param name="skeleton">skeleton to draw bones from</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        /// <param name="jointType0">joint to start drawing from</param>
        /// <param name="jointType1">joint to end drawing at</param>
        private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
        {
            Joint joint0 = skeleton.Joints[jointType0];
            Joint joint1 = skeleton.Joints[jointType1];

            // If we can't find either of these joints, exit
            if (joint0.TrackingState == JointTrackingState.NotTracked ||
                joint1.TrackingState == JointTrackingState.NotTracked)
            {
                return;
            }

            // Don't draw if both points are inferred
            if (joint0.TrackingState == JointTrackingState.Inferred &&
                joint1.TrackingState == JointTrackingState.Inferred)
            {
                return;
            }

            // We assume all drawn bones are inferred unless BOTH joints are tracked
            Pen drawPen = this.inferredBonePen;
            if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
            {
                drawPen = this.trackedBonePen;
            }

            drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
        }

        /// <summary>
        /// Handles the checking or unchecking of the seated mode combo box
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void CheckBoxSeatedModeChanged(object sender, RoutedEventArgs e)
        {
            if (null != this.sensor)
            {
                if (this.checkBoxSeatedMode.IsChecked.GetValueOrDefault())
                {
                    this.sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated;
                }
                else
                {
                    this.sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
                }
            }
        }

         private String createBoneVectorString(Skeleton skeleton) {
            //CHILD COORD THEN PARENT COORD TO GENERATE BONE VECTOR
            StringBuilder s = new StringBuilder();
             StringBuilder inferString = new StringBuilder();
            s.Append(calculateRotation(JointType.ShoulderRight, JointType.ShoulderLeft, skeleton, "ROTA"));
   //         s.Append(calculateBoneVector(JointType.ShoulderCenter, JointType.Spine, skeleton, "SCSp"));
            //s.Append(calculateBoneVector(JointID.ShoulderLeft, JointID.ShoulderCenter, data, "SLSC"));
            //s.Append(calculateBoneVector(JointID.ShoulderRight, JointID.ShoulderCenter, data, "SRSC"));
            //s.Append(calculateBoneVector(JointID.Head, JointID.ShoulderCenter, data, "HdSC"));
            s.Append(calculateBoneVectorMessage(JointType.ElbowLeft, JointType.ShoulderLeft, skeleton, "ELSL"));
            s.Append(calculateBoneVectorMessage(JointType.ElbowRight, JointType.ShoulderRight, skeleton, "ERSR"));
            s.Append(calculateBoneVectorMessage(JointType.WristLeft, JointType.ElbowLeft, skeleton, "WLEL"));
            s.Append(calculateBoneVectorMessage(JointType.WristRight, JointType.ElbowRight, skeleton, "WRER"));
     //       s.Append(calculateBoneVector(JointType.ShoulderCenter, JointType.Head, skeleton, "HORI"));
            //s.Append(calculateBoneVector(JointID.HandLeft, JointID.WristLeft, data, "HLWL"));
            //s.Append(calculateBoneVector(JointID.KneeLeft, JointID.HipLeft, data, "KLHL"));
            //s.Append(calculateBoneVector(JointID.KneeRight, JointID.HipRight, data, "KRHR"));


            //JointID j = JointID.ShoulderCenter;
            //JointID k = JointID.Head;

    //        Console.WriteLine((data.Joints[j].Position.X - data.Joints[k].Position.X) + "," + (data.Joints[j].Position.Y - data.Joints[k].Position.Y) + "," + (data.Joints[j].Position.Z - data.Joints[j].Position.X));

            return s.ToString();
        }

        private String calculateBoneVectorMessage(JointType i, JointType j, Skeleton d, String header) {
            float[] initVec = new float[3];
            float[] unNormVec = new float[3];
            float[] normVec = new float[3];

            String inferFlag = "";
            if (d.Joints[i].TrackingState == JointTrackingState.NotTracked && d.Joints[j].TrackingState == JointTrackingState.NotTracked) {
                inferFlag = "*";
            }
            else if (d.Joints[i].TrackingState == JointTrackingState.Inferred || d.Joints[j].TrackingState == JointTrackingState.Inferred) {
                inferFlag = "&";
            }

            initVec[0] = d.Joints[i].Position.X - d.Joints[j].Position.X;
            initVec[1] = d.Joints[i].Position.Y - d.Joints[j].Position.Y;
            initVec[2] = d.Joints[i].Position.Z - d.Joints[j].Position.Z;

            if (isCalibrated) {
                 unNormVec = this.getCalibratedVector(initVec);
            }
            else
            {
                unNormVec = initVec;
            }

            double vectorLength = Math.Sqrt((unNormVec[0] * unNormVec[0]) + (unNormVec[1] * unNormVec[1]) + (unNormVec[2] * unNormVec[2]));
            normVec[0] = (float)(unNormVec[0] / vectorLength);
            normVec[1] = (float)(unNormVec[1] / vectorLength);
            normVec[2] = (float)(unNormVec[2] / vectorLength);

            return inferFlag + header + ":X" + Convert.ToString(normVec[0]) + "Y" + Convert.ToString(normVec[1]) + "Z" + Convert.ToString(normVec[2] + "!");
        }

        private float[] calculateBoneVector(JointType i, JointType j, Skeleton d)
        {
            float[] initVec = new float[3];
            float[] unNormVec = new float[3];
            float[] normVec = new float[3];

            initVec[0] = d.Joints[i].Position.X - d.Joints[j].Position.X;
            initVec[1] = d.Joints[i].Position.Y - d.Joints[j].Position.Y;
            initVec[2] = d.Joints[i].Position.Z - d.Joints[j].Position.Z;

            if (isCalibrated)
            {
                unNormVec = this.getCalibratedVector(initVec);
            }
            else
            {
                unNormVec = initVec;
            }

            double vectorLength = Math.Sqrt((unNormVec[0] * unNormVec[0]) + (unNormVec[1] * unNormVec[1]) + (unNormVec[2] * unNormVec[2]));
            normVec[0] = (float)(unNormVec[0] / vectorLength);
            normVec[1] = (float)(unNormVec[1] / vectorLength);
            normVec[2] = (float)(unNormVec[2] / vectorLength);

            return normVec;
        }

        private String calculateRotation(JointType i, JointType j, Skeleton d, String header) {
            
            System.Windows.Vector v1 = new System.Windows.Vector(1, 0);
            System.Windows.Vector v2 = new System.Windows.Vector(d.Joints[i].Position.X - d.Joints[j].Position.X, d.Joints[i].Position.Z - d.Joints[j].Position.Z);
            v2.Normalize();
            double a = Math.Atan2(v2.Y, v2.X) - Math.Atan2(v1.Y, v1.X);
            double angle = -(a * (180.0 / Math.PI));
            String inferFlag = createRotationInferString(d.Joints[i].TrackingState, d.Joints[j].TrackingState);
            return inferFlag + header + ":" + Math.Round(angle, 3) + "!";
        }

        private String createRotationInferString(JointTrackingState i, JointTrackingState j) { 
            String inferFlag = "";
            if (i == JointTrackingState.NotTracked && j == JointTrackingState.NotTracked) {
                inferFlag = "*";
            }
            else if (i == JointTrackingState.Inferred || j == JointTrackingState.Inferred) {
                inferFlag = "&";
            }
            return inferFlag;
        }

        private float[] getCalibratedVector(float[] initVec) {
            float[] calibVec = new float[3];

            calibVec[0] = initVec[0];
            calibVec[1] = initVec[1] * (float)Math.Cos(elevationAngle) - initVec[2] * (float)Math.Sin(elevationAngle);
            calibVec[2] = initVec[1] * (float)Math.Sin(elevationAngle) + initVec[2] * (float)Math.Cos(elevationAngle);

            return calibVec;
        
        }

        private void recordButtonClick(object sender, RoutedEventArgs e){
           
            if (recordButton.Content.Equals("Start Recording"))
            {
                if (writer == null)
                {
                    writer = new StreamWriter("KayoTest.txt");
                }
                recording = true;
                recordButton.Content = "STOP";
            }
            else {
                recording = false;
                recordButton.Content = "Start Recording";
                writer.WriteLine("***********");
            }
        }

        public float[] getGestureData(Skeleton s) {

            List<float[]> vecs = new List<float[]>();
            float[] data = null;

            if (s != null)
            {
                vecs.Add(calculateBoneVector(JointType.ShoulderLeft, JointType.ShoulderCenter, s));
                vecs.Add(calculateBoneVector(JointType.ElbowLeft, JointType.ShoulderLeft, s));
                vecs.Add(calculateBoneVector(JointType.WristLeft, JointType.ElbowLeft, s));
                vecs.Add( calculateBoneVector(JointType.ShoulderRight, JointType.ShoulderCenter, s));
                vecs.Add(calculateBoneVector(JointType.ElbowRight, JointType.ShoulderRight, s));
                vecs.Add(calculateBoneVector(JointType.WristRight, JointType.ElbowRight, s));

                data = new float[vecs.Count * 3];
                int i = 0;

                foreach (float[] vec in vecs) {
                    foreach (float val in vec) {
                        data[i] = val;
                        i++;
                    }
                }

            }
            return data;
        }


        public void recordToFile(float[] data)
        {
            StringBuilder s = new StringBuilder();

            foreach (float f in data) {
                s.Append(f + ",");
            }
            writer.WriteLine(s);
        }

    }
}